
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Detector } from "./Detector";
import { DetectorResult } from "../DetectorResult";
import { Geometry } from "../../geometry/Geometry";
import { Transform } from "../../../common/Transform";
import { CachedDetectorData } from "./CachedDetectorData";
import { SphereGeometry } from "../../geometry/SphereGeometry";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { MathUtil } from "../../../common/MathUtil";
import { BoxGeometry } from "../../geometry/BoxGeometry";

/**
 * Sphere vs Box collision detector.
 */

export class SphereBoxDetector extends Detector {
	/**
	 * If `swapped` is `true`, the collision detector expects `BoxGeometry` and `SphereGeometry` for the
	 * first and second argument of `SphereBoxDetector.detect`. If `swapped` is `false`, the collision detector expects
	 * `SphereGeometry` and `BoxGeometry` instead.
	 */
	constructor(swapped: boolean) {
		super(swapped);
	}

	public detectImpl(result: DetectorResult, geom1: Geometry, geom2: Geometry, tf1: Transform, tf2: Transform, cachedData: CachedDetectorData): void {
		var s: SphereGeometry = geom1 as SphereGeometry;
		var b: BoxGeometry = geom2 as BoxGeometry;

		result.incremental = false;

		var halfExt: Vec3 = b._halfExtents.clone();
		var negHalfExt: Vec3 = halfExt.negate();

		var r: number = s._radius;

		var boxToSphere: Vec3 = new Vec3();
		M.vec3_sub(boxToSphere, tf1._position, tf2._position);

		var boxToSphereInBox: Vec3 = boxToSphere.mulMat3(tf2._rotation);

		// is the center of the sphere inside the box?
		var insideBox: boolean = M.aabb_overlap(negHalfExt, halfExt, boxToSphereInBox, boxToSphereInBox);
		if (insideBox) {
			// compute minimum distance between the center of the sphere and the box's surfaces that are perpendicular to each axis
			var sphereToBoxSurface: Vec3 = boxToSphereInBox.abs();
			M.vec3_sub(sphereToBoxSurface, halfExt, sphereToBoxSurface);

			var normalInBox: Vec3 = new Vec3();

			// ... and select the smallest one, setting normal
			var distX: number = M.vec3_get(sphereToBoxSurface, 0);
			var distY: number = M.vec3_get(sphereToBoxSurface, 1);
			var distZ: number = M.vec3_get(sphereToBoxSurface, 2);
			var depth: number;
			var projectionMask: Vec3 = new Vec3();
			switch (M.compare3min(distX, distY, distZ)) {
				case -1:
					{
						if (M.vec3_get(boxToSphereInBox, 0) > 0) {
							normalInBox = new Vec3(1, 0, 0)
						} else {
							normalInBox = new Vec3(-1, 0, 0);
						}
						projectionMask = new Vec3(0, 1, 1);
						depth = distX;
					}
					break;
				case 0:
					{
						if (M.vec3_get(boxToSphereInBox, 1) > 0) {
							normalInBox = new Vec3(0, 1, 0);
						} else {
							normalInBox = new Vec3(0, -1, 0);
						}
						projectionMask = new Vec3(1, 0, 1);
						depth = distY;
					}
					break;
				case 1:
					{
						if (M.vec3_get(boxToSphereInBox, 2) > 0) {
							normalInBox = new Vec3(0, 0, 1);
						} else {
							normalInBox = new Vec3(0, 0, -1);
						}
						projectionMask = new Vec3(1, 1, 0);
						depth = distZ;
					}
					break;
			}

			// compute the closest point
			var base: Vec3 = new Vec3();
			M.vec3_compWiseMul(base, projectionMask, boxToSphereInBox);
			var boxToClosestPointInBox: Vec3 = new Vec3();
			M.vec3_compWiseMul(boxToClosestPointInBox, normalInBox, halfExt);
			M.vec3_add(boxToClosestPointInBox, boxToClosestPointInBox, base);

			// bring them back to the world coordinate system
			var boxToClosestPoint: Vec3 = new Vec3();
			var normal: Vec3 = new Vec3();
			M.vec3_mulMat3(boxToClosestPoint, boxToClosestPointInBox, tf2._rotation);
			M.vec3_mulMat3(normal, normalInBox, tf2._rotation);
			this.setNormal(result, normal);

			var pos1: Vec3 = new Vec3();
			var pos2: Vec3 = new Vec3();
			M.vec3_addRhsScaledTo(pos1, tf1._position, normal, -r);
			M.vec3_add(pos2, tf2._position, boxToClosestPoint);
			this.addPoint(result, pos1, pos2, depth, 0);
			return;
		}

		// compute the closest point to the center of the sphere; just clamp the coordinate
		var boxToClosestPointInBox: Vec3 = new Vec3();

		// avoid division by zero
		var eps: number = 1e-9;
		var epsVec: Vec3 = new Vec3(eps, eps, eps);
		M.vec3_sub(halfExt, halfExt, epsVec);
		M.vec3_add(negHalfExt, negHalfExt, epsVec);

		M.vec3_clamp(boxToClosestPointInBox, boxToSphereInBox, negHalfExt, halfExt);

		var closestPointToSphereInBox: Vec3 = new Vec3();
		M.vec3_sub(closestPointToSphereInBox, boxToSphereInBox, boxToClosestPointInBox);

		var dist: number = M.vec3_dot(closestPointToSphereInBox, closestPointToSphereInBox);
		if (dist >= r * r) {
			// does not collide
			return;
		}
		dist = MathUtil.sqrt(dist);

		// bring them back to the world coordinate system
		var boxToClosestPoint: Vec3 = new Vec3();
		var closestPointToSphere: Vec3 = new Vec3();
		M.vec3_mulMat3(boxToClosestPoint, boxToClosestPointInBox, tf2._rotation);
		M.vec3_mulMat3(closestPointToSphere, closestPointToSphereInBox, tf2._rotation);

		var normal: Vec3 = closestPointToSphere.normalized();
		this.setNormal(result, normal);

		var pos1: Vec3 = new Vec3();
		var pos2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(pos1, tf1._position, normal, -r);
		M.vec3_add(pos2, tf2._position, boxToClosestPoint);
		this.addPoint(result, pos1, pos2, r - dist, 0);
	}
}
